#include <ros/ros.h>
#include <ros/time.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <cmath>
#include <deque>
#include <thread>


struct imuPoint
{
    Eigen::Vector3d pos;    // 位置
    Eigen::Matrix3d orien;  // 姿态 旋转矩阵
    Eigen::Vector3d w;      // 角速度
    Eigen::Vector3d v;      // 线速度
};


class ImuOdom
{
private:
    std::string imu_Topic = "/imu/data";
    std::string imu_Odom_Topic = "/imu/odom";

    nav_msgs::Odometry odom;
    
    ros::Time time;
    imuPoint point;
    Eigen::Vector3d gravity;

    double imu_time = 0;
    double deltaT;
    int initCount = 0;
    double initGx = 0;
    double initGy = 0;
    double initGz = 0;
    bool firstT;

public:

    ros::NodeHandle nh;
    ros::Subscriber subImu;     // 订阅IMU话题
    ros::Publisher pubImuOdom;  // 发布IMU里程计


    
    ImuOdom()
    {
        subImu = nh.subscribe<sensor_msgs::Imu>(imu_Topic, 1000, &ImuOdom::ImuCallback, this, ros::TransportHints().tcpNoDelay());
        pubImuOdom = nh.advertise<nav_msgs::Odometry>(imu_Odom_Topic, 1000, true);
        
        Eigen::Vector3d zero(0, 0, 0);
        point.pos = zero;
        point.orien = Eigen::Matrix3d::Identity();
        point.v = zero;
        point.w = zero;
        firstT = true;
    
    }


    /**************************************************************
     * @brief imu的回调函数处理
     * 
     * 1.接受订阅的imu话题
     * 2.处理重力加速度
     * 3.积分计算位姿的旋转，速度和位置
     * 4.发布更新的imu里程计
     * 
     * @param msgIn 订阅的imu数据指针
     *************************************************************/
    void ImuCallback(const sensor_msgs::Imu::ConstPtr& msgIn)
    {
        sensor_msgs::Imu msg = *msgIn;

        imu_time = msg.header.stamp.toSec();    // 记录imu数据的时间戳

        if (firstT)
        {
            time = msg.header.stamp;
            deltaT = 0;
            setGravity(msg.linear_acceleration);    // 将第一帧的加速度作为重力加速度
            // firstT = false;
        }
        else
        {
            deltaT = (msg.header.stamp - time).toSec(); // 当前帧和上一帧的时间差
            time = msg.header.stamp;   
            // odom.header.seq = msg.header.seq;
            // odom.header.stamp = msg.header.stamp;

            calcOrientation(msg.angular_velocity);  // 计算角度，旋转向量表示
            calcPosition(msg.linear_acceleration);  // 计算速度和位置
            publish_odom(point);                    // 发布更新的imu里程计
            
        }
    }


    /**************************************************************
     * @brief 设置重力加速度的值
     * 
     * @param msg imu的加速度向量值
     *************************************************************/
    void setGravity(const geometry_msgs::Vector3 &msg) 
    {
        initGx += msg.x;
        initGy += msg.y;
        initGz += msg.z;
        initCount ++;
        
        if (initCount == 1000)
        {
            initGx = initGx / 1000;
            initGy = initGy / 1000;
            initGz = initGz / 1000;

            gravity[0] = initGx;
            gravity[1] = initGy;
            gravity[2] = initGz;

            firstT = false;

            std::cout << gravity[0] << std::endl;
            std::cout << gravity[1] << std::endl;
            std::cout << gravity[2] << std::endl;
        }
 
        // gravity[0] = msg.x;
        // gravity[1] = msg.y;
        // gravity[2] = msg.z;
    }

    /**************************************************************
     * @brief 计算位姿的旋转
     * 
     * @param msg imu的角速度向量值
     *************************************************************/
    void calcOrientation(const geometry_msgs::Vector3 &msg)
    {
        point.w << msg.x, msg.y, msg.z;

        // B: 旋转向量的反对称矩阵
        // msg.*dt = angle;  
        Eigen::Matrix3d B;
        B << 0, -msg.z * deltaT, msg.y * deltaT, 
            msg.z * deltaT, 0, -msg.x * deltaT,
            -msg.y * deltaT, msg.x * deltaT, 0;
        
        // sigma: 旋转向量的模长
        double sigma =
            std::sqrt(std::pow(msg.x, 2) + std::pow(msg.y, 2) + std::pow(msg.z, 2)) *
            deltaT;
        // std::cout << "sigma: " << sigma << std::endl << Eigen::Matrix3d::Identity()
        // + (std::sin(sigma) / sigma) * B << std::endl << pose.orien << std::endl;

        // 罗德里格斯公式: 
        // I + (1-cos(θ))*B*B^T + sin(θ)*B = I - (1-cos(θ))*B*B + sin(θ)*B; 
        // 注: 反对称矩阵 B^T = -B
        point.orien = point.orien *
                    (Eigen::Matrix3d::Identity() + (std::sin(sigma) / sigma) * B -
                        ((1 - std::cos(sigma)) / std::pow(sigma, 2)) * B * B);
    }

    /**************************************************************
     * @brief 计算位置
     * 
     * 1.将imu坐标系下的加速度左乘旋转向量，转换到odom坐标系下
     * 2.在odom坐标系下的加速度扣除重力加速度的分量，得到纯加速度的变化量
     * 3.根据加速度进行积分计算速度和位姿
     * @param msg 
     *************************************************************/
    void calcPosition(const geometry_msgs::Vector3 &msg)
    {
        Eigen::Vector3d acc_l(msg.x, msg.y, msg.z);     // imu坐标系下的加速度
        Eigen::Vector3d acc_g = point.orien * acc_l;    // odom坐标系下的加速度
        
        // v = v + dt * (a - g);
        // p = p + dt * v;
        point.v = point.v + deltaT * (acc_g - gravity); // 积分得到速度
        point.pos = point.pos + deltaT * point.v;       // 积分得到位置
    }

    /**************************************************************
     * @brief 发布更新imu里程计
     * 
     * @param point 
     *************************************************************/
    void publish_odom(const imuPoint point)
    {
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";
        odom.header.stamp = ros::Time().fromSec(imu_time);
        
        //位置
        odom.pose.pose.position.x = point.pos(0);
        odom.pose.pose.position.y = point.pos(1);
        odom.pose.pose.position.z = point.pos(2);

        //姿态 四元数
        odom.pose.pose.orientation.x = (point.orien(2,1) - point.orien(1,2)) / 4; 
        odom.pose.pose.orientation.y = (point.orien(0,2) - point.orien(2,0)) / 4;
        odom.pose.pose.orientation.z = (point.orien(1,0) - point.orien(0,1)) / 4;
        odom.pose.pose.orientation.w = std::sqrt(1 + point.orien(0,0) + point.orien(1,1) + point.orien(2,2)) / 2;
        
        //线速度
        odom.twist.twist.linear.x = point.v(0);
        odom.twist.twist.linear.y = point.v(1);
        odom.twist.twist.linear.z = point.v(2);

        //角速度
        odom.twist.twist.angular.x = point.w(0);
        odom.twist.twist.angular.y = point.w(1);
        odom.twist.twist.angular.z = point.w(2);

        //发布里程计
        pubImuOdom.publish(odom);
    }




    void processIMUOdom()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }
    }


};



int main(int argc, char** argv)
{
    ros::init(argc, argv, "IMUOdom");

    ImuOdom MO;

    ROS_INFO("\033[1;32m----> IMU Odom Procee Started.\033[0m");

    std::thread IMUOdomProcessThread(&ImuOdom::processIMUOdom, &MO);

    ros::spin();

    IMUOdomProcessThread.join();

    return 0;
}
